﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Net.Sockets;
using System.Net;

    class KinectMachineThread
    {
        private bool isConnected;
        private TcpClient client;
        private String jointPositionString = "";
        private String inferredJoints = "";
        private int angularPosition;
        private int baseAngularPosition;
        private float rotationAngle = 0;
        private NetworkStream clientStream;
        private int id;
        private KinectNetworkManager parentManager;

        public KinectMachineThread(KinectNetworkManager knm, TcpClient c, int newID)
        {

            client = c;
            clientStream = client.GetStream();

            ASCIIEncoding encoder = new ASCIIEncoding();
            byte[] receivedMessage = new byte[4096];
            int bytesRead = 0;
            id = newID;

            try
            {
                bytesRead = clientStream.Read(receivedMessage, 0, 4096);
            }
            catch
            {
                Console.WriteLine("Client has disconnected");
            }

            char[] message = new char[bytesRead];

            for (int i = 0; i < bytesRead; i++)
            {
                message[i] = Convert.ToChar(receivedMessage[i]);
            }

            String s = new String(message);

            if (s.StartsWith("P"))
            {
                s = s.Remove(0, 1);
                baseAngularPosition = Convert.ToInt32(s);
                angularPosition = baseAngularPosition;
            }
            else { Console.WriteLine(s); }

            Console.WriteLine("Machine connected at angle of " + angularPosition);

            parentManager = knm;

            isConnected = true;

            Thread t = new Thread(new ThreadStart(runThread));
            t.IsBackground = true;
            t.Start();
        }

        private void runThread()
        {

            while (isConnected)
            {
                this.getClientData();
            }

            Console.WriteLine("closing...");

            clientStream.Flush();
            clientStream.Close();
            client.Close();

        }

        //gets update from machine
        public void getClientData()
        {

            ASCIIEncoding encoder = new ASCIIEncoding();
            byte[] sentMessage = new byte[0];
            byte[] receivedMessage = new byte[4096];

            string s = "";

            try
            {
                sentMessage = encoder.GetBytes("0");
                clientStream.Write(sentMessage, 0, sentMessage.Length);
                clientStream.Flush();

                int bytesRead = 0;

                bytesRead = clientStream.Read(receivedMessage, 0, 4096);
                s = ASCIIEncoding.ASCII.GetString(receivedMessage, 0, bytesRead);
            }

            catch
            {
                Console.WriteLine("Client has disconnected");
                parentManager.removeMachineClient(id);
                isConnected = false;
            }

            jointPositionString = s;
            this.setRotationAngle();

        }

        //finds rotation angle and sets a message
        public void setRotationAngle()
        {

            if (!jointPositionString.Equals("") && !jointPositionString.Equals(";"))
            {
                try
                {
                    int start = jointPositionString.IndexOf("ROTA:") + 5;
                    int end = jointPositionString.IndexOf("!");
                    String s = jointPositionString.Substring(start, end - start);

                    float f = float.Parse(s);

                    //limits angle
                    if (Math.Abs(f) < 75) {
                        rotationAngle = f;
                    }
                }
                catch (Exception e)
                {
                    Console.WriteLine("cant parse " + jointPositionString);
                    rotationAngle = -100000;
                }
            }
            else
            {
                rotationAngle = -100000;
            }
        }

        public float getRotationAngle()
        {
            return rotationAngle;
        }

        public String getJointPositionString()
        {
            return jointPositionString;
        }


        public void setAngularPosition(int i) {
            
            //only change if new position is not far from original
            if (Math.Abs(i - this.baseAngularPosition) < 20)
            {
                angularPosition = i;
            }
            else
            {

                Console.WriteLine(i + "is too much");
            }
        
        }

        public void resetAngularPosition() {
            angularPosition = this.baseAngularPosition;
        }

        public int getBaseAngularPosition() {
            return baseAngularPosition;
        }

        public int getAngularPosition()
        {
            return angularPosition;
        }

        public void setID(int i)
        {
            id = i;
        }

        public int getID()
        {
            return id;
        }


    }
